﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading;

namespace comm
{
    public class AStarNode
    {
        public int X { get; set; }
        public int Y { get; set; }
        public int F { get; set; }
        public int G { get; set; }
        public int H { get; set; }
        public AStarNode Parent { get; set; }
        public objst.DirType DirType { get; set; }

        public override string ToString()
        {
            return X + "," + Y;
        }
    }

    public class AStarNodeSort : IComparer<AStarNode>
    {
        public int Compare(AStarNode arg0, AStarNode arg1)
        {
            if (arg0.F == arg1.F)
            {
                return 0;
            }
            else if (arg0.F > arg1.F)
            {
                return 1;
            }
            return -1;
        }
    }

    public class AStar
    {
        static Dictionary<objst.DirType, int[][]> _dirXDic = new Dictionary<objst.DirType, int[][]>();
        static List<int[]> _rDots = new List<int[]>();
        static AStar()
        {
            _dirXDic.Add(objst.DirType.TOP_LEFT, new int[][] { new int[] { 0, -1 }, new int[] { -1, 0 } });
            _dirXDic.Add(objst.DirType.TOP_RIGHT, new int[][] { new int[] { 0, -1 }, new int[] { 1, 0 } });
            _dirXDic.Add(objst.DirType.BOTTOM_LEFT, new int[][] { new int[] { -1, 0 }, new int[] { 0, 1 } });
            _dirXDic.Add(objst.DirType.BOTTOM_RIGHT, new int[][] { new int[] { 1, 0 }, new int[] { 0, 1 } });

            _rDots.AddRange(new int[][] { new int[] { 0, -1 } });
            _rDots.AddRange(new int[][] { new int[] { -1, 0 }, new int[] { 1, 0 } });
            _rDots.AddRange(new int[][] { new int[] { 0, 1 } });
        }
        static int[][] GetDirXDots(objst.DirType dir)
        {
            return _dirXDic[dir];
        }

        public bool CheckAngle { get; private set; }

        List<AStarNode> _openList = new List<AStarNode>();
        Dictionary<string, AStarNode> _openDic = new Dictionary<string, AStarNode>();
        Dictionary<string, AStarNode> _closeDic = new Dictionary<string, AStarNode>();
        AStarNodeSort _sort = new AStarNodeSort();

        AStarNode _curr = null;
        bool _find = false;
        int _ex = 0, _ey = 0;
        int _w = 0, _h = 0, _pathSize = 0;
        Func<int, int, int, bool> _checkPath = null;

        public AStar(bool checkAngle = true)
        {
            CheckAngle = checkAngle;
        }

        public AStarNode[] Find(int pathSize, int w, int h, int sx, int sy, int ex, int ey, Func<int, int, int, bool> checkPath)
        {
            _pathSize = pathSize;
            _w = w;
            _h = h;
            _checkPath = checkPath;

            var rs = new List<AStarNode>();
            _openList.Clear();
            _openDic.Clear();
            _closeDic.Clear();
            _find = false;
            _ex = ex;
            _ey = ey;

            if (CheckAngle)
            {
                if (!CehckR(sx, sy) || !CehckR(ex, ey))
                {
                    return rs.ToArray();
                }
            }

            var n = new AStarNode { X = sx, Y = sy };
            _openList.Add(n);
            _openDic.Add(n.X + "," + n.Y, n);

            while (_openList.Count > 0)
            {
                var node = _openList[0];
                _curr = node;
                _openDic.Remove(node.X + "," + node.Y);
                _openList.RemoveAt(0);
                _closeDic.Add(node.X + "," + node.Y, node);
                if (_find)
                {
                    var nd = _curr;
                    rs.Add(nd);
                    while (nd.Parent != null)
                    {
                        nd = nd.Parent;
                        rs.Add(nd);
                    }
                    rs.Reverse();
                    break;
                }

                Round();
                _openList.Sort(_sort);
            }

            return rs.ToArray();
        }

        bool CehckR(int x, int y)
        {
            var count = 0;
            foreach (var dot in _rDots)
            {
                var bx = dot[0] + x;
                var by = dot[1] + y;
                if (_checkPath(bx, by, _pathSize))
                {
                    count++;
                    break;
                }
            }
            return count > 0;
        }
        void AddOpenList(int x, int y, int g, objst.DirType dir)
        {
            var key = x + "," + y;
            if (x > _w - 1 || y > _h - 1 || !_checkPath(x, y, _pathSize) || _closeDic.ContainsKey(key))
            {
                return;
            }
            if (CheckAngle && g == 14)
            {
                var dots = GetDirXDots(dir);
                if (!_checkPath(_curr.X + dots[0][0], _curr.Y + dots[0][1], _pathSize) &&
                    !_checkPath(_curr.X + dots[1][0], _curr.Y + dots[1][1], _pathSize))
                {
                    return;
                }
            }

            if (!CheckOpen(x, y, g, dir))
            {
                int hx = (int)Math.Abs(x - _ex);
                int hy = (int)Math.Abs(y - _ey);

                var node = new AStarNode
                {
                    X = x,
                    Y = y,
                    F = 0,
                    G = _curr.G + g,
                    H = 10 * (hx + hy),
                    Parent = _curr,
                    DirType = dir
                };
                node.F = node.G + node.H;
                _openList.Add(node);
                _openDic.Add(node.X + "," + node.Y, node);
            }

            if (x == _ex && y == _ey)
            {
                _find = true;
                return;
            }
        }
        bool CheckOpen(int x, int y, int g, objst.DirType dir)
        {
            if (_openDic.ContainsKey(x + "," + y))
            {
                var node = _openDic[x + "," + y];
                if (node.G > _curr.G + g)
                {
                    node.Parent = _curr;
                    node.G = _curr.G + g;
                    node.F = node.G + node.H;
                    node.DirType = dir;
                }
                return true;
            }
            return false;
        }
        void Round()
        {
            int x = _curr.X, y = _curr.Y,
             xA = x + 1, yA = y + 1,
             xJ = x - 1, yJ = y - 1;

            bool bTop = false, bLeft = false, bRight = false, bBottom = false;

            // top
            if (yJ >= 0)
            {
                bTop = true;

                // x,yJ

                AddOpenList(x, yJ, 10, objst.DirType.TOP);
            }

            // bottom
            if (yA <= _h)
            {
                bBottom = true;

                // x,yA

                AddOpenList(x, yA, 10, objst.DirType.BOTTOM);
            }

            // left
            if (xJ >= 0)
            {
                bLeft = true;

                // xJ,y

                AddOpenList(xJ, y, 10, objst.DirType.LEFT);
            }

            // right
            if (xA <= _w)
            {
                bRight = true;

                // xA,y

                AddOpenList(xA, y, 10, objst.DirType.RIGHT);
            }

            // top left
            if (bTop && bLeft)
            {
                // xJ,yJ

                AddOpenList(xJ, yJ, 14, objst.DirType.TOP_LEFT);
            }

            // top right
            if (bTop && bRight)
            {
                // xA,yJ

                AddOpenList(xA, yJ, 14, objst.DirType.TOP_RIGHT);
            }

            // bottom left
            if (bBottom && bLeft)
            {
                // xJ,yA

                AddOpenList(xJ, yA, 14, objst.DirType.BOTTOM_LEFT);
            }

            // bottom right
            if (bBottom && bRight)
            {
                // xA,yA

                AddOpenList(xA, yA, 14, objst.DirType.BOTTOM_RIGHT);
            }
        }
    }
}
